#include <iostream>
#include <vector>
#include <numeric>
#include <opencv2/opencv.hpp>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/point_stamped.hpp"

double offset_correction_x = 0, offset_correction_y = 0;
cv::Mat trans_std2map;

#define TRANS_READ
// #define TRANS_WRITE
/**
 * @brief 获取点云地图到云台手地图的平移和旋转矩阵
 * 
 * @param dst_point_set 目标点集
 * @param src_point_set 源点集
 * @param R 旋转矩阵
 * @param T 平移矩阵
 */
void getCoordinateTransformationMatrix(std::vector<cv::Point2f> src_point_set, std::vector<cv::Point2f> dst_point_set,cv::Mat &transform)
{
    // 检查输入
    if(dst_point_set.size() < 3 || src_point_set.size() < 3)
    {
        std::cout << "getCoordinateTransformationMatrix输入不满足条件" << 
            dst_point_set.size() << " " <<  src_point_set.size() << std::endl;
        return;
    }
    if(dst_point_set.size() != src_point_set.size())
    {
        auto size = std::min(src_point_set.size(), dst_point_set.size());
        dst_point_set.resize(size);
        src_point_set.resize(size);
    }

    transform = cv::getPerspectiveTransform(src_point_set, dst_point_set);

    // // 开始计算
    // cv::Mat src(src_point_set);  // 云台手点
    // cv::Mat dst(dst_point_set);    // 点云点
    // src = src.reshape(1, 0);
    // dst = dst.reshape(1, 0);
    // std::cout << "size: " << src.size() << "  " << dst.size() << std::endl;
    // std::cout << src << std::endl << dst << std::endl;
    // // 计算重心点
    // cv::Point3d src_cm(0., 0., 0.);
    // cv::Point3d dst_cm(0., 0., 0.);
    // for (int i = 0; i < src.rows; i++)
    // {#ifdef TRANS_READ
    //     cv::Point3d p(src.at<double>(i, 0), src.at<double>(i, 1), src.at<double>(i, 2));
    //     src_cm += p;
    // }
    // for (int i = 0; i < dst.rows; i++)
    // {
    //     cv::Point3d q(dst.at<double>(i, 0), dst.at<double>(i, 1), dst.at<double>(i, 2));
    //     dst_cm += q;
    // }
    // src_cm /= src.rows;
    // dst_cm /= dst.rows;
    // // 将源点集和目标点集平移到重心处
    // cv::Mat src_centered = src - cv::Mat::ones(src.rows, 1, CV_64F) * cv::Mat(src_cm).t();
    // cv::Mat dst_centered = dst - cv::Mat::ones(dst.rows, 1, CV_64F) * cv::Mat(dst_cm).t();

    // // 计算协方差矩阵
    // cv::Mat W = cv::Mat::zeros(3, 3, CV_64F);
    // for (int i = 0; i < src.rows; i++)
    // {
    //     cv::Mat pi = src_centered.row(i).t();
    //     cv::Mat qi = dst_centered.row(i).t();
    //     W += qi * pi.t();
    // }

    // // 进行奇异值分解
    // cv::Mat U, S, Vt;
    // cv::SVD::compute(W, S, U, Vt, cv::SVD::MODIFY_A);

    // // 计算旋转矩阵和平移矩阵
    // R = Vt.t() * U.t();
    // T = cv::Mat(dst_cm) - R * cv::Mat(src_cm);

    return;
}


auto loadPointCloudMap(std::string file_path)
{
    cv::Mat pgm_img = cv::imread(file_path, cv::IMREAD_UNCHANGED);
    cv::Mat normalized_img;
    
    normalize(pgm_img, normalized_img, 0, 255, cv::NORM_MINMAX);

    normalized_img.convertTo(normalized_img, CV_8U);
    return normalized_img;
}

auto loadClientMap(std::string file_path)
{
    return cv::imread(file_path);
}

bool mouse_click_flag = false;
int mouse_x = 0;
int mouse_y = 0;
void mouse_callback(int event, int x, int y, int flag, void* params)
{
    if(event == cv::EVENT_LBUTTONDOWN)  //左键按下
    {
        mouse_click_flag = true;
        mouse_x = x;
        mouse_y = y;
    }
}

auto getMouseClicks()
{
    return cv::Point2i(mouse_x, mouse_y);
}

auto mouseClicked()
{
    if(mouse_click_flag)
    {
        mouse_click_flag = false;
        return true;
    }
    return false;
}

// void operator<<(std::vector<cv::Point3d> &dst, std::vector<cv::Point2d> src)
// {
//     for(auto point : src)#ifdef TRANS_READ
//         cv::Point3d p(point.x, point.y, 0);
//         dst.push_back(p);
//     }
//     return;
// }

void operator<<(std::vector<cv::Point2f> &dst, std::vector<cv::Point2i> src)
{
    for(auto point : src)
    {
        cv::Point2f p(point.x, point.y);
        dst.push_back(p);
    }
    return;
}

std::vector<cv::Point2f> PerspectiveTransform(cv::Size client_map_size, std::vector<cv::Point2i> client_point_set)
{
    // 1.根据云台手地图和实际地图的比例关系，计算出图片上的点到实际点的坐标变换关系
    // 点顺序：从左下角开始，顺时针4个顶点
    std::vector<cv::Point2f> default_client_map_points = {cv::Point2f(0, client_map_size.height), cv::Point2f(0, 0), 
                     cv::Point2f(client_map_size.width, client_map_size.height), cv::Point2f(client_map_size.width, 0)};
    std::vector<cv::Point2f> default_real_map_points = {cv::Point2f(0, 0), cv::Point2f(0, 15), cv::Point2f(28, 0), cv::Point2f(28, 15)};
    auto perspectiveTransformMatrix_client2realMap = getPerspectiveTransform(default_client_map_points, default_real_map_points);
    // 2.将图片上的点转换成实际点
    std::vector<cv::Point2f> real_map_points;
    std::vector<cv::Point2f> client_map_points;
    client_map_points << client_point_set;
    cv::perspectiveTransform(client_map_points, real_map_points, perspectiveTransformMatrix_client2realMap);
    return real_map_points;
}


std::vector<cv::Point2f> goal_pose_points;
#ifdef TRANS_WRITE
void getGoalPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
    cv::Point2f p(msg->pose.position.x, msg->pose.position.y);
    goal_pose_points.push_back(p);
}
#endif
#ifdef TRANS_READ
void generateMapPoint(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
    cv::Point2f p(msg->pose.position.x - offset_correction_x, msg->pose.position.y - offset_correction_y);
    std::vector<cv::Point2f> in_p = {p}, out_p;
    cv::perspectiveTransform(in_p, out_p, trans_std2map.inv());
    std::cout<< "point on std map: \n(" <<out_p[0].x << ", " << out_p[0].y << ")\n";
}
#endif

rclcpp::Publisher<geometry_msgs::msg::PointStamped>::SharedPtr pub;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub;

void main_thread()
{
#ifdef TRANS_WRITE
    // auto point_cloud_map = loadPointCloudMap("/path/to/PointClouldMap");
    auto client_map = loadClientMap(MAP_STD_SRC_PATH);
    if(client_map.empty())
    {
        std::cout << "云台手小地图图片为空" << std::endl;
        return;
    }
    std::vector<cv::Point2i> client_point_set;

    //! point_count : 要点的点个数
    int point_count = 4;    
    int i = 0;
    
    cv::namedWindow("point cloud map");
    cv::setMouseCallback("point cloud map", mouse_callback, NULL);
    // 可视化标点 
    while (i < point_count)
    {
        cv::imshow("point cloud map", client_map);
        if(mouseClicked())
        {
            auto point = getMouseClicks();
            client_point_set.push_back(point);
            ++i;
            std::cout << "Point " << point << std::endl;
            cv::circle(client_map, point, 3, cv::Scalar(0, 255, 255));
        }
        if(cv::waitKey(10) == 'q') exit(0);
    }
    cv::imshow("point cloud map", client_map);
    std::cout << "图片采点结束" << std::endl;
    // 等rviz标出足够多的点
    while(goal_pose_points.size() < point_count) 
    {
        if(cv::waitKey(10) =='q') exit(0);
        if(!rclcpp::ok()) exit(0);
    };
    std::cout << "rviz采点结束" << std::endl;

    // 从像素坐标变换到云台手坐标
    auto real_points_set = PerspectiveTransform(client_map.size(), client_point_set);
    // 获得转换矩阵
    // 1.从云台手小地图坐标点变到导航地图坐标点

    getCoordinateTransformationMatrix(goal_pose_points, real_points_set,trans_std2map);
    std::cout << "trans_std2map\n" << trans_std2map << std::endl;
    // fix
    std::vector<cv::Point2f> check_points;
    cv::perspectiveTransform(real_points_set, check_points, trans_std2map);
    for(int k = 0; k < point_count; k ++)
    {
        offset_correction_x += goal_pose_points[k].x - check_points[k].x;
        offset_correction_y += goal_pose_points[k].y - check_points[k].y;
    }
    offset_correction_x /= point_count;
    offset_correction_y /= point_count;
    std::cout << "check_points\n" << check_points << std::endl;
    std::cout << "offset_correction_x: " << offset_correction_x << std::endl;
    std::cout << "offset_correction_y: " << offset_correction_y << std::endl;
    //保存
    cv::FileStorage map_config(MAP_TRANSFORMATION_PATH, cv::FileStorage::WRITE);
    // map_config << "client2cloud" << "{";
    map_config << "trans_std2map" << trans_std2map;
    map_config << "offset_correction_x" << offset_correction_x;
    map_config << "offset_correction_y" << offset_correction_y;
    // map_config << "T" << cv::Mat::zeros(3, 1, CV_64F);
    // map_config << "}";
    map_config.release();
    // 2.从导航地图坐标点变到云台手小地图坐标点
    // cv::Mat R_cloud2client, T_cloud2client;
    // getCoordinateTransformationMatrix(real_points_set, goal_pose_points, R_cloud2client, T_cloud2client);
    // std::cout << "R_cloud2client\n" << R_cloud2client << "\nT_cloud2client\n" << T_cloud2client << std::endl;
    //保存
    // cv::FileStorage map_config(MAP_TRANSFORMATION_PATH, cv::FileStorage::WRITE);
    // map_config << "client2cloud" << "{";
    // map_config << "R" << R_client2cloud;
    // map_config << "T" << T_client2cloud;
    // map_config << "}";
    // map_config << "cloud2client" << "{";
    // map_config << "R" << R_cloud2client;
    // map_config << "T" << T_cloud2client;
    // map_config << "}";
    // map_config.release();
    
    // 3.从导航地图坐标点变到云台手小地图坐标点
    // cv::Mat R_map2client, T_map2client;
    // getCoordinateTransformationMatrix(real_points_set, check_points, R_map2client, T_map2client);
    int j = 0;
    while (true)
    {
        int key = cv::waitKey(0);
        if(key == 'n')
        {
            cv::Point2d check_point = check_points[j];
            geometry_msgs::msg::PointStamped point;
            point.header.frame_id = "map";
            point.header.stamp = rclcpp::Clock().now();
            point.point.x = check_point.x + offset_correction_x;
            point.point.y = check_point.y + offset_correction_y;
            point.point.z = 0;
            pub->publish(point);
            if(++j > real_points_set.size()-1) j = 0;
        }
        if(key == 'q') break;
    }
#endif


#ifdef TRANS_READ

    cv::FileStorage map_config_read(MAP_TRANSFORMATION_PATH, cv::FileStorage::READ);
    map_config_read["trans_std2map"] >> trans_std2map;
    map_config_read["offset_correction_x"] >> offset_correction_x;
    map_config_read["offset_correction_y"] >> offset_correction_y;

    // cv::Mat R_cloud2client, T_cloud2client;
    // map_config["cloud2client"]["R"] >> R_cloud2client;
    auto client_map = loadClientMap(MAP_STD_SRC_PATH);
    cv::imshow("point cloud map", client_map);
    cv::setMouseCallback("point cloud map", mouse_callback, NULL);
    while (true)
    {
        if(mouseClicked())
        {
            auto point = getMouseClicks();
            // client_map = loadClientMap(MAP_STD_SRC_PATH);
            cv::circle(client_map, point, 3, cv::Scalar(0, 255, 255));
            std::vector<cv::Point2i> client_p;
            client_p.push_back(point);
            std::vector<cv::Point2f> real_p = PerspectiveTransform(client_map.size(), client_p);
            std::cout << "real_p:\n" << real_p << std::endl;
            std::vector<cv::Point2f> check_points;
            cv::perspectiveTransform(real_p, check_points, trans_std2map);
            // std::cout << "check_points\n" << check_points << std::endl;
            cv::imshow("point cloud map", client_map);
            
            geometry_msgs::msg::PointStamped point_msg;
            point_msg.header.frame_id = "map";
            point_msg.header.stamp = rclcpp::Clock().now();
            point_msg.point.x = check_points[0].x + offset_correction_x;
            point_msg.point.y = check_points[0].y + offset_correction_y;
            point_msg.point.z = 0;
            std::cout << "point pub\n";
            pub->publish(point_msg);
        }
        if(cv::waitKey(10) == 'q') exit(0);
    }

    
#endif
}

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>("goal_pose_client");
#ifdef TRANS_WRITE
    sub = node->create_subscription<geometry_msgs::msg::PoseStamped>("/goal_pose", 10, std::bind(getGoalPoseCallback, std::placeholders::_1));
#endif
#ifdef TRANS_READ
    sub = node->create_subscription<geometry_msgs::msg::PoseStamped>("/goal_pose", 10, std::bind(& generateMapPoint, std::placeholders::_1));
#endif
    pub = node->create_publisher<geometry_msgs::msg::PointStamped>("/point", 10);
    std::thread t(main_thread);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}